#include "queue.h"
#include <string.h>

/**
 * \file queue.c
 * \author Clecio Varjao and Leanne Ross
 * \date October 2007
 * \brief Implementation of a Simple and Circular Queue
 */



#define MAX_QUEUE 1
#define MAX_QUEUE_ITEM 3

volatile void *NULLPTR=NULL;
volatile unsigned int ZERO = 0;

volatile QUEUE				Queues[MAX_QUEUE];	//!< Array of QUEUE objects
volatile QUEUE_ITEM			QueueItems[MAX_QUEUE_ITEM];	//!< Array of QUEUE_ITEM objects
volatile unsigned int QCount=0;
volatile unsigned int QICount=0;


/*!
 * Check if an 'object' was sucessefully allocated
 */
void check_alloc(void *o, unsigned char where){
	if (o==NULLPTR){
		//OS_Error(101);
	}
}

/*!
 * Return a new QUEUE_ITEM 'object'
 */
QUEUE_ITEM *queue_item_alloc(){	
	volatile QUEUE_ITEM *ret;
	unsigned int i;

	ret=NULLPTR;
	i=ZERO;
	if (QICount<MAX_QUEUE_ITEM){
		for (; i< MAX_QUEUE_ITEM; i++){
			if (QueueItems[i].allocated==0){
				ret=(QUEUE_ITEM*)&QueueItems[i];
				memset((void*)ret,0,sizeof(QUEUE_ITEM));
				ret->allocated=1;
				QICount++;
				break;
			}
		}
	}
	check_alloc((void*)ret, 2);
	return (QUEUE_ITEM*)ret;
}
/*!
 * Release an QUEUE_ITEM 'object'
 */
void queue_item_free(QUEUE_ITEM *o){
	o->allocated=0;
	QICount--;
}

/*!
 * Return a new QUEUE 'object'
 */
QUEUE *queue_alloc(){
	unsigned int i;
	volatile QUEUE *ret;

	ret=NULLPTR;
	i=ZERO;

	if (QCount<MAX_QUEUE){
		for (; i< MAX_QUEUE; i++){
			if (Queues[i].allocated==0){
				ret=(QUEUE*)&Queues[i];
				memset((void*)ret,0,sizeof(QUEUE));
				ret->allocated=1;
				QCount++;
				break;
			}
		}
	}
	check_alloc((void*)ret, 3);
	return (QUEUE*)ret;
}

/*!
 * Release an QUEUE 'object'
 */
void queue_free(QUEUE *q){
	volatile QUEUE_ITEM *t1, *t2;
	t1=q->next;
	while(t1!=NULLPTR){
		t2=t1->next;
		queue_item_free((QUEUE_ITEM*)t1);
		t1=t2;
	}
	q->allocated=0;
	QCount--;
}


/*!
 *\brief Create a new Queue
 *\param isCircular Indicathe if the queue will be circular (0/1)
 */
QUEUE *queue_create(unsigned char isCircular){
	QUEUE *ret;

	ret=queue_alloc();
	ret->isCircular=isCircular;
	return ret;
}


/*!
 *\brief Remove the item \c p from the queue \c q
 *\param q Queue to be manipulated
 *\param p Item to be removed
 */
void queue_remove(QUEUE *q, void *p){
	QUEUE_ITEM *aux;
	QUEUE_ITEM *prev;
	
	aux=q->first;
	prev=0;

	if (aux!=0){
		do{
			if (aux->item==p){				
				if (prev!=0){
					prev->next=aux->next;
				}
				
				if (aux==q->next){
					q->next=aux->next;
					if (aux==q->next){ // if persists, which means circular
						q->next=0;
					}
				}
				
				if (q->first==aux){ //First Item
					q->first=q->next;
				}
				if (q->last==aux){ // Last item
					q->last=q->next;
				}
				
				queue_item_free(aux);
				break;
			}
			prev=aux;
			aux=aux->next;
		}while (aux!=0 || aux!=q->first);

	}
}

/*!
 *\brief Enqueue item \c o in the queue \c q
 *\param q Queue to be manipulated
 *\param o Item to be inserted
 *
 *Insert an item to the end of the queue
 */
void enqueue(QUEUE *q, void *o){
	QUEUE_ITEM *newItem;

	newItem=queue_item_alloc();

	newItem->prev=q->last;
	newItem->item=o;

	if (q->first==0){ //EMPTY queue
		q->first=newItem;
		q->next=newItem;
	}else{
		q->last->next=newItem;		
	}

	q->last=newItem;

	if (q->isCircular==1){ //connect head/tail
		q->last->next=q->first;
		q->first->prev=q->last;
	}
}

/*!
 *\brief Dequeue an from a queue
 *\param q Queue to be manipulated
 *
 *Return the Next item in the queue. If it is a simple queue, the queue_item is destroyed, otherwise just move the \c next pointer.
 */
void *dequeue(QUEUE *q){
	QUEUE_ITEM *cur;
	void *ret;

	ret=0;
	cur=q->next;
	if (cur!=0){ // if there is a next item
		q->next=cur->next; // Move to next
		ret=cur->item;
		if (q->isCircular==0){
			q->first=q->next;
			if (q->last==cur){
				q->last=NULL;
			}
			queue_item_free(cur);	// Release Memory
		}
	}
	return ret;
}

/*!
 *\brief Remove an specific queue_item from the queue
 *\param q Queue to be manipulated
 *\param item Queue_item to be removed
 *
 *Remove the \c item from \c q. If it is not circular, the queue_item object is destroyed.
 */
void queue_remove_at(QUEUE *q, QUEUE_ITEM *item){
	QUEUE_ITEM *p;
	QUEUE_ITEM *n;

	if (q->isCircular){
		q->next=item->next;
	} else {
		p=item->prev;
		n=item->next;
		if (p!=0){
			p->next=n;
		}
		if (n!=0){
			n->prev=p;
		}
		if (item==q->next){
			q->next=0;
		}
		if (item==q->first){
			q->first=0;
		}
		if (item==q->last){
			q->last=0;
		}
		
		queue_item_free(item);	// Release Memory
		
	}
}
